//
//  State.m
//  Racing
//
//  Created by vy phan on 4/14/11.
//  Copyright 2011 __MyCompanyName__. All rights reserved.
//

#import "State.h"
#import "Body.h"

@implementation State

static inline void arraycopy(NSMutableArray* src, int srcPos, NSMutableArray* dest, int destPos, int length) 
{
	if ([src count] < srcPos + length || [dest count] < destPos + length) {
		return;
	}
	for (int i = srcPos, j = destPos; i < srcPos + length; i++, j++) {
		[dest replaceObjectAtIndex:j withObject:[src objectAtIndex:i]];
	}
}

-(id) init
{
	//	public float x = 0;
	//	public float y = 0;
	//	
	//	public float[] xpoints = null;
	//	public float[] ypoints = null;
	//	public int numpoints = -1;
	//	
	//	public float vx = 0;
	//	public float vy = 0;
	//	protected float fx = 0;
	//	protected float fy = 0;
	//	
	//	public float angle = (float)(0);
	//	public float angularVelocity = 0.0f;
	//	protected float torque = 0;
	self = [super init];
	if (self != nil) {
		self->x = 0;
		self->y = 0;
		self->xpoints = [[NSMutableArray alloc]init];
		self->ypoints = [[NSMutableArray alloc]init];
		numpoints = -1;
		vx = 0.0f;
		vy = 0.0f;
		fx = 0.0f;
		fy = 0.0f;
		angle = 0.0f;
		angularVelocity = 0.0f;
		torque = 0.0f;
		colliding = NO;
		cx = 0;
		cy = 0;
		nx = 0;
		ny = 0;
		numcollisions = 0;
		mass = 1;
		momentOfInertia = 1;
	}
	return self;
}

-(id) initWithOuterBody:(Body*)outer_
{
	//	public float x = 0;
	//	public float y = 0;
	//	
	//	public float[] xpoints = null;
	//	public float[] ypoints = null;
	//	public int numpoints = -1;
	//	
	//	public float vx = 0;
	//	public float vy = 0;
	//	protected float fx = 0;
	//	protected float fy = 0;
	//	
	//	public float angle = (float)(0);
	//	public float angularVelocity = 0.0f;
	//	protected float torque = 0;
	self = [super init];
	if (self != nil) {
		self->x = 0;
		self->y = 0;
		self->xpoints = [[NSMutableArray alloc]init];
		self->ypoints = [[NSMutableArray alloc]init];
		numpoints = -1;
		vx = 0.0f;
		vy = 0.0f;
		fx = 0.0f;
		fy = 0.0f;
		angle = 0.0f;
		angularVelocity = 0.0f;
		torque = 0.0f;
		colliding = NO;
		cx = 0;
		cy = 0;
		nx = 0;
		ny = 0;
		numcollisions = 0;
#ifdef USE_BODY_OBJ
		self->outer = outer_;
		//self->outer = [[[[Body alloc]init] retain] autorelease];
#else
		mass = 1;
		momentOfInertia = 1;
#endif
	}
	return self;
}
/* collid with another object or not
 */
-(BOOL) isColliding
{
	return colliding;
}
/* calculate speed of car
 */
-(float) speed
{
	return (float)sqrt(vx*vx + vy*vy);
}
/* apply force to an object (tac dung luc vao doi tuong)
 */
-(void) applyForce:(float)x_ Y:(float)y_ FX:(float)fx_ FY:(float)fy_
{
	self->fx = self->fx + fx_;
	self->fy = self->fy + fy_;
	
	// adjust torque
	float rx = (x_ - self->x);
	float ry = (y_ - self->y);
	
	torque -= (-ry*fx_ + rx*fy_);
}
/** Apply an impulse (collision force) to the Body,
 *  at x, y with the collision normal nx, ny.
 **/
-(void) collide:(float)x_ Y:(float)y_ NX:(float)nx_ NY:(float)ny_
{
	float rx = (x - self->x);
	float ry = (y - self->y);
	
	// velocity of point
	float vx = self->vx + angularVelocity*ry;
	float vy = self->vy + -angularVelocity*rx;
	
	// if we are heading away already, don't do anything
	if ( nx_*vx + ny_*vy > 0 )
		return;
	
	cx += x_;
	cy += y_;
	self->nx += nx_;
	self->ny += ny_;
	numcollisions++;
	
	colliding = YES;
}

/** Apply an impulse (collision force) to the Body,
 *  at x, y with the collision normal nx, ny.
 **/
-(void) collide:(float)x Y:(float)y NX:(float)nx NY:(float)ny Body:(Body*)body State:(State*)state  
{
	colliding = YES;
	state->colliding = YES;
	// not done yet
	// needs to work out speed at collision point
	
	/*float rx1 = (x - this.x);
	 float ry1 = (y - this.y);
	 float rx2 = (x - state.x);
	 float ry2 = (y - state.y);*/
	
	
	
	// velocities
	float vx1 = self->vx;// + angularVelocity*ry1;
	float vy1 = self->vy;// + -angularVelocity*rx1;
	float vx2 = state->vx;// + state.angularVelocity*ry2;
	float vy2 = state->vy;// + -state.angularVelocity*rx2;
	
	
	// velocity of point
	float vx = vx1 - vx2;
	float vy = vy1 - vy2;
		#ifdef USE_BODY_OBJ
	float impulseSize = -1.1f * ( outer.mass + body.mass );
#else
	float impulseSize = -1.1f * ( mass + body.mass );
#endif
	float impulsefx = impulseSize * abs( nx ) * vx;
	float impulsefy = impulseSize * abs( ny ) * vy;
	
	if ( nx*vx + ny*vy > 0 )
		return;
	#ifdef USE_BODY_OBJ
	float massInv = 1.0f / (outer.mass + outer.momentOfInertia);
#else
	float massInv = 1.0f / (mass + momentOfInertia);
#endif
	float ax = impulsefx*massInv;
	float ay = impulsefy*massInv;
	
	self->vx += ax;
	self->vy += ay;
	
	state->vx -= ax;
	state->vy -= ay;
	
	
	//System.out.println( "ax " + ax + " ay " + ay );
	//System.out.println( "vx " + state.vx + " vy " + state.vy );
}

-(void) computeCollisions 
{
	
	if ( numcollisions == 0 )
		return;
	
	cx /= numcollisions;
	cy /= numcollisions;
	nx /= numcollisions;
	ny /= numcollisions;
	
	float rx = (cx - self->x);
	float ry = (cy - self->y);
	
	// velocity of point
	float vx = self->vx + angularVelocity * ry;
	float vy = self->vy + (-angularVelocity * rx);
	
	// if we are heading away already, don't do anything
	if ( nx*vx + ny*vy > 0 )
		return;
	
	
#ifdef USE_BODY_OBJ
	float impulseSize = -1.1f * outer.mass;
#else
	float impulseSize = -1.1f * mass;

#endif
	
	float impulsefx = impulseSize * fabs( nx ) * vx;
	float impulsefy = impulseSize * fabs( ny ) * vy;
	
	float impulseTorque = -( -ry * impulsefx + rx * impulsefy );
		#ifdef USE_BODY_OBJ
	float ax = impulsefx / outer.mass;
	float ay = impulsefy / outer.mass;
#else
	float ax = impulsefx / mass;
	float ay = impulsefy / mass;
#endif
	float friction = 0.2f;
	self->vx += (ax - friction * self->vx * fabs( ny ));  // friction
	self->vy += (ay - friction * self->vy * fabs( nx ));
	#ifdef USE_BODY_OBJ
	angularVelocity += (float)(impulseTorque / outer.momentOfInertia);
#else
	angularVelocity += (float)(impulseTorque / momentOfInertia);
#endif
	if ( angularVelocity > M_PI ) {
		angularVelocity = (float)M_PI;
	}
	else if ( angularVelocity < -M_PI ) {
		angularVelocity = -(float)M_PI;
	}
	
	numcollisions = 0;
	cx = cy = nx = ny = 0;
	
	colliding = NO;
}

-(void) copy:(State*)state 
{
	self->x = state->x;
	self->y = state->y;
	
	self->vx = state->vx;
	self->vy = state->vy;
	self->fx = state->fx;
	self->fy = state->fy;
	
	self->angle = state->angle;
	self->angularVelocity = state->angularVelocity;
	self->torque = state->torque;
	self->colliding = state->colliding;
#ifdef USE_BODY_OBJ
	self->outer = state->outer;
#endif
	arraycopy( state->xpoints, 0, xpoints, 0, numpoints );
	arraycopy( state->ypoints, 0, ypoints, 0, numpoints );
	
	//NSLog(@">>>>>>>>>>>>>>> vehicle state update");
}

/** Stop all movement, zero all forces. **/
-(void) halt
{
	vx = vy = 0.0f;
	fx = fx = 0.0f;
	angularVelocity = 0.0f;
	torque = 0.0f;
}

/** Integrate this state to get the state after dt seconds.
 *  <CODE>next</CODE> will contain the new state.
 **/
-(void) integrate:(float)dt NextState:(State*)next  
{
	
	// integrate linear (positional) stuff
	next->x = x + dt * vx;
	next->y = y + dt * vy;
	
	torque -= 200.0f * angularVelocity;
	[self applyForce:x Y:y FX:-1.5f * vx FY:-1.5f * vy];
#ifdef USE_BODY_OBJ
	float ax = fx / outer.mass;
	float ay = fy / outer.mass;
#else
	float ax = fx / mass;
	float ay = fy / mass;
#endif
	next->vx = vx + dt * ax;
	next->vy = vy + dt * ay;
	
	float speedSq = next->vx * next->vx + next->vy * next->vy;
	
	int maxSpd = 50;
	if ( speedSq > maxSpd*maxSpd ) {
		float speed = sqrtf( speedSq );
		next->vx = (maxSpd * next->vx/speed);
		next->vy = (maxSpd * next->vy/speed);
	}
	
	// integrate angle
	next->angle = angle + dt * angularVelocity;
	if (next->angle != next->angle) {
		NSLog(@"NAN");
	}
	#ifdef USE_BODY_OBJ
	next->angularVelocity = angularVelocity + dt * torque/outer.momentOfInertia;
	// other angle stuff
#else
	next->angularVelocity = angularVelocity + dt * torque/momentOfInertia;
#endif
	
	
	next->fx = 0.0f;
	next->fy = 0.0f;
	next->torque = 0.0f;
	
	next->colliding = NO;
	
}
-(void)dealloc
{
#ifdef USE_BODY_OBJ
	[outer release];
#endif
	[self->xpoints release];
	[self->ypoints release];
	[super dealloc];
}
@end
